#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "navigation.h"
#include "datamanger.h"
#include "odom.h"
#include "control.h"
#include "robot.h"
#include "path.h"

const uint8_t direction_after_turnleft[4] = {3, 2, 0, 1};      //对应转弯后的方向
const uint8_t direction_after_turnright[4] = {2, 3, 1, 0};
const uint8_t direction_after_turnback[4] = {1, 0, 3, 2};       

static uint8_t is_stop_station(uint8_t station_index);
static int32_t turning_in_radius(uint8_t radius);

void nvidia_handle(uint8_t n_devicename, DATA32_t n_devicedata) 
{
	int16_t Robot_Offset;                   //小车偏移量
	uint16_t cross_distance = 0;            //停靠点测得距离
    int32_t around_speed;
	static uint8_t outline_flag = 0;    
    ROBOT_POSITION cur_position;            //获取当前的最新里程计数据  

    ACTION_TYPE act_type;                   //当前十字停靠点需要作的动作
	
	switch(n_devicename)
	{
		case NVIDIA_OFFSET:		//巡线功能s
            if(robotinfo_read(LINE_WARNING)){
                around_speed = robotinfo_read(AROUND_SPEED);        //获取速度
                if(((around_speed > 0) && (n_devicedata.raw[0] == 0)) || ((around_speed < 0) && (n_devicedata.raw[0] == 1)))
                {
                    Robot_Offset = (n_devicedata.raw[2] << 8) | n_devicedata.raw[3];				
                    Robot_PID_Ctrl(robotinfo_read(AROUND_SPEED), Robot_Offset);
                }
                else{   //摄像头与行进方向不一致，需要切换摄像头方向
                    if(around_speed > 0)    navigation_camera_ctrl(1);
                    else if(around_speed < 0)   navigation_camera_ctrl(0);
                    printf("camera direction error! around speed: %d\n", around_speed);
                }
                //printf("direction: %d\n",n_devicedata.raw[0]);
            }
		    break;
        case NVIDIA_DINGZI:
        case NVIDIA_SHIZI:
            read_odom(&cur_position);       //获取当前的最新里程计数据  
            cross_distance = n_devicedata.raw[3]; 
            if(cross_distance == 0) return;

            

            if((cur_position.dist_to_radpoint > (navigation.distance - 60)) 
            && (cur_position.dist_to_radpoint < (navigation.distance + 60)) )
            {
                printf("cross_distance: %d - odom: %d - duan: %d\n", cross_distance, cur_position.dist_to_radpoint, navigation.distance);
                if(((navigation.cur_station == 2) || (navigation.cur_station == 28) || (navigation.cur_station == 49) || (navigation.cur_station == 54))
                        && ((navigation.next_station == 1) || (navigation.next_station == 27) || (navigation.next_station == 48) || (navigation.next_station == 53)))
                {
                    //从1、三、5、6支路返回到根节点
                    act_type = get_path_station();  //返回0表示到达需要停靠的点
                    if (act_type == TURNRIGHT)  //回充电点
                    {
                        trigger_event(EVENT_TURN_AROUND, -90, cross_distance, 0);   //因为需要退回充电房，所以是反向转弯，转弯之后继续巡线，速度为负
                    }   
                    else if(act_type == TURNLEFT)   //继续下一个支路巡检
                    {
                        trigger_event(EVENT_TURN_AROUND, -90, cross_distance, 1);    //

                    }
                }
                else if(((navigation.cur_station == 25) || (navigation.cur_station == 45) )
                        && ((navigation.next_station == 26) || (navigation.next_station == 46)))
                {
                    //从2、4支路返回主路，机器人正向运动
                    act_type = get_path_station();  //返回0表示到达需要停靠的点
                    if (act_type == TURNLEFT)  //回充电点
                    {
                        trigger_event(EVENT_TURN_AROUND, 90, cross_distance, 1);   //因为需要退回充电房，所以是反向转弯，转弯之后反向继续巡检
                    }   
                    else if(act_type == TURNRIGHT)   //继续下一个支路巡检
                    {
                        trigger_event(EVENT_TURN_AROUND, 90, cross_distance, 0);    //

                    }
                }
                else{   
                    act_type = get_path_station();  //返回0表示到达需要停靠的点
                    if(act_type ==STOP)  
                    {
                        trigger_event(EVENT_GO_DISTANCE, cross_distance, 0, 0);
                        //开始计时，停靠点设置等待时间
                    }
                    else if(act_type == FORWARD){ 
                        write_odom(0,0);
                        //圆弧转弯需要切换方向
                        if(((navigation.cur_station == 13) || (navigation.cur_station == 38)) && ((navigation.next_station == 14) || (navigation.next_station == 39)))
                        {
                            navigation.robot_direction = P_WEST;
                            navigation.run_direction = P_WEST;
                        }
                        if((navigation.cur_station == 37) && (navigation.next_station == 38))
                        {
                            navigation.robot_direction = P_NORTH;
                            navigation.run_direction = P_NORTH;
                        }
                    }
                    else if(act_type == BACKWARD){
                        write_odom(0,0);
                        printf("nvidia_handle-action :backward!\n");
                    }
                    else if(act_type == TURNRIGHT){
                        if(((navigation.cur_station == 26) || (navigation.cur_station == 46)) 
                            && ((navigation.next_station == 25) || (navigation.next_station == 45)))    //进入2、4岔路，需要左传反向
                        {
                            trigger_event(EVENT_TURN_AROUND, -90, cross_distance, 1);   //左传后切换行进方向为向后,附加里程计清零，消除里程计转弯时的计数
                        }
                        else
                        {
                            trigger_event(EVENT_TURN_AROUND, 90, cross_distance, 0);    //转弯之后继续巡线
                        }
                        
                    }
                    else if(act_type == TURNLEFT){
                        trigger_event(EVENT_TURN_AROUND, -90, cross_distance, 0);
                    }
                    else{
                        printf("error : nvidia_handle!\n");
                    }
                }
            }			
            break;  
// 		case NVIDIA_OUTLINE:
// 			break;
// 		default:
// 			break;
	}
}


/**
 * @brief 自动巡检模式下启动
 * 
 * @param speed 
 */
void auto_arount(int32_t speed)
{
    ROBOT_POSITION  cur_position;
    ACTION_TYPE action;
    int next_sta;

    if(robotinfo_read(WORK_MODE) == 0)
    {    
        read_odom(&cur_position);       //获取当前的里程计数据

        next_sta = get_next_station();             //获取下一个巡检点，并作路径规划
        //printf("next station: %d \n", next_sta);
        action = get_path_station();        //规划好的路径中查找下一个点

        robotinfo_write(WORK_MODE, 1);          //机器人开始工作
        switch (action)
        {
            case FORWARD:
                robotinfo_write(AROUND_SPEED, speed);   //写入速度
                robotinfo_write(LINE_WARNING, 1);       //开启巡线
                break;
            case BACKWARD:
                navigation.run_direction = direction_after_turnback[navigation.run_direction];      //切换机器人的行进方向
                write_odom(0, 90);                      //切换方向计算车身距离
                robotinfo_write(AROUND_SPEED, -speed);   //写入速度
                robotinfo_write(LINE_WARNING, 1);       //开启巡线
                break;                            
            default:    
                printf("auto_arount: curent no station;; action: %d!\n", action);
                robotinfo_write(WORK_MODE, 0);          //机器人停止工作
                break;
        }

    }


}


void go_distance_handler(void)
{
    int32_t cont20ms = 0;
    ROBOT_POSITION cur_val;  
    int32_t target_val = get_event_targetval();
    int32_t follow_act = get_event_followact();
    printf("distance to go: %d; follow_act: %d\n", target_val, follow_act);
    read_odom(&cur_val);
    if(target_val < 0){         //区分前进还是后退
        navigation_camera_ctrl(0);              //切换摄像头
        robotinfo_write(AROUND_SPEED, -80);
    }
    else
    {
        navigation_camera_ctrl(1);
        robotinfo_write(AROUND_SPEED, 80);      
    }
    target_val = abs(target_val) + cur_val.dist_to_radpoint;
    robotinfo_write(LINE_WARNING, 1);       //打开巡线
    printf("target value: %d\n", target_val);
    while(1)
    {   
        if(cont20ms < 500)              //时间限制：10s
        {
            read_odom(&cur_val);    
            
            if(target_val <= cur_val.dist_to_radpoint)
            {
                switch(follow_act){
                    case 0: //停下  
                        RobotActionCTRL(FORWARD, 0);
                        robotinfo_write(AROUND_SPEED, 0);
                        robotinfo_write(LINE_WARNING, 0);  
                        robotinfo_write(WORK_MODE, 0);          //机器人停止工作
                        write_odom(0,0);
                        //eth_cmd_package(0 , THE_INFO,7,cur_val.dual);         //到达后上报信号给上位机
                        printf("go distance arrived  %d \n",cur_val.dual);
                    break;
                    case 1:

                    break;
                    default:
                    break;
                }
                //event 结束
                clear_event();
                break;
            }
        }
        else{                           //定点调整超时，停止，上报错误        
            cont20ms = 0;
            RobotActionCTRL(FORWARD, 0);    //停止巡线，停下
            clear_event();                  //清除当前事件
            printf("err: go_distance_handler out times!\n");

        }
        usleep(20000);   //20ms一次
        
    }

}

/**
 * @brief 转弯处理，线程调用
 * 
 */
void turn_around_handler(void)
{
    int32_t cont20ms = 0;
    int32_t speed_l, speed_r, around_speed;     //左轮、右轮、巡检

    int32_t cur_val = 0;
    int32_t target_val;
    int32_t follow_act;
    int32_t action_param;
    int8_t turn_dir = 0;        //转弯方向，临时变量

    write_odom(0,0);        //转弯后，里程计清零
    RobotActionCTRL(FORWARD, 0);            //停止巡线，停下，，关闭巡线
    usleep(500000);                         //等待停下，500ms

    cur_val = robotinfo_read(GYRO);         //获取当前陀螺仪值
    target_val = get_event_targetval();     //转动角度增加5度
    action_param = get_action_param();      //转弯半径
    follow_act = get_event_followact();     //获取event数据

    if(target_val > 0){                     //右转
        turn_dir = 1;
        speed_r = 200;
        speed_l = turning_in_radius(action_param + 55);     //车身长80，车中心则为一半：40
        target_val = (cur_val - target_val) < -180 ? (cur_val - target_val + 360) : (cur_val - target_val);      //判断绕回        

    }
    else if(target_val < 0){                //左传
        turn_dir = -1;
        speed_l = 200;
        speed_r = turning_in_radius(action_param + 55);
        target_val = (cur_val - target_val) > 180 ? (cur_val - target_val - 360) : (cur_val - target_val);      //判断绕回

    }
    else{                                   //target_val为0, 返回
        return; 
    }

    around_speed = robotinfo_read(AROUND_SPEED);    //读取巡检速度
    
    RobotEachControl((around_speed / abs(around_speed)) * speed_r,(around_speed / abs(around_speed)) * speed_l);  //写入速度，根据巡检速度区分前转向、后转向
    printf("cur_val: %d, target_val: %d\n",cur_val,target_val);
    cont20ms = 0;
    while(1)
    {
        cur_val = robotinfo_read(GYRO);     //获取当前陀螺仪值
        if(cont20ms < 800)                  //时间限制：16s
        {
            if(abs(cur_val - target_val) < 10)  //角度差小于10度
            {
                //奇幻方向
                if(turn_dir > 0){
                    navigation.robot_direction = direction_after_turnright[navigation.robot_direction];     //切换方向
                    navigation.run_direction = direction_after_turnright[navigation.run_direction];
                }
                else if(turn_dir < 0){
                    navigation.robot_direction = direction_after_turnleft[navigation.robot_direction];      //切换方向
                    navigation.run_direction = direction_after_turnleft[navigation.run_direction];
                }   

                switch (follow_act)
                {
                    case 0:     //正常巡检转向
                        write_odom(0,action_param + 130);    //转弯后，里程计为转弯半径
                        break;
                    case 1:     //转向后，巡检速度取反

                        robotinfo_write(AROUND_SPEED, -around_speed);   //转弯之后都是前进速度，曲绝对值
                        navigation.run_direction = direction_after_turnback[navigation.run_direction];  //切换行进方向
                        if(around_speed > 0)
                        {
                            navigation_camera_ctrl(0);      //切换摄像头
                        }else{
                            navigation_camera_ctrl(1);      //切换摄像头
                        }
                        write_odom(0,0);        //转弯后，里程计清零
                        break;
                    default:
                        printf("turn_around_handler: follow_act error!\n");
                        break;
                }
                robotinfo_write(LINE_WARNING, 1);   //使能巡线
                
                
                
                clear_event();                                      //清除当前事件
                printf("arrived\n");
                return;
            }       
        }
        else{                           //旋转超时，停止，上报错误        
            cont20ms = 0;
            RobotActionCTRL(FORWARD, 0);    //停止巡线，停下
            clear_event();                  //清除当前事件
            printf("err: turn_around_handler out times!\n");
            return;
        }
        
        cont20ms++;
        usleep(20000);  //20ms
    }

}

/**
 * @brief 按某个半径转弯
 * 
 * @param radius ：转弯半径
 * @return :转弯内侧轮子速度（cm/s）
 */
static int32_t turning_in_radius(uint8_t radius)
{
    int32_t speed;
    if(radius < 25)             //轴距2， 这个地方算25
    {
        speed = 0;              //转弯半径小，内侧轮子速度为0
    }
    else if(radius < 180)       //防止错误的值
    {
        //speed = (int32_t) ((((radius - 30) * 20) / (radius + 30)) * 10);   //轴距30， 外侧轮速20cm/s(keya：200)
        speed = (int32_t) (((radius - 24) * 200) / (radius + 24));
    }
    else{
        printf("turn_around_handler: radius error!\n");
        return -1;
    }
    return speed;
}

/**
 * @brief 机器人切换方向
 * 
 */

void robot_turn_back(void)
{

}